/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.subsystems;

import edu.wpi.first.wpilibj.Victor;
import edu.wpi.first.wpilibj.command.Subsystem;
import edu.wpi.first.wpilibj.templates.commands.TestableCommand;

/**
 *
 * @author Jarrod Risley
 */
public class DifferentialSubsystem extends Subsystem {
    
    /*
     * Global Class Variables
     */
    private static DifferentialState currentState;
    private Victor leftShifterVictor;
    private Victor rightShifterVictor;

    private String subsystemName;
    
    public static class DifferentialState {
        
        /*
         * Global Subclass variables
         */
        private double value;
        private static final double lowGear = 1;
        private static final double highGear = -1;
        
        public static final DifferentialState LOW_GEAR =
                new DifferentialState(lowGear);
        
        public static final DifferentialState HIGH_GEAR = 
                new DifferentialState(highGear);
        
        // SUBCLASS CONSTRUCTOR //
        
        
        private DifferentialState(double state) {
            value = state;
        } // end DifferentialState Constructor
        
        
        // SUBCLASS METHODs //
        
        
        /**
         * Since <code>value</code> is private, this method will grab the 
         * DifferentialState's value.
         * 
         * @return The DifferentialState's value
         */
        public double getValue() {
            return value;
        } // end getValue
        
        /**
         * Prints out which state we are in.
         * 
         * @return A string representation 
         */
        public String toString() {
            
            if (currentState.getValue() == 1)
                return "Low Gear";
            else if (currentState.getValue() == -1)
                return "High Gear";
            
            return "";
            
        } // end toString
        
    } // end DIfferentialState subclass
    
    
    // CONSTRUCTOR //
    
    
    /**
     * Creates a DifferentialSubsystem.
     * 
     * @param name The name of the subsystem.
     * @param sidecarNum The sidecar the subsystem is attached to.
     * @param pwmPort The port to which the servos are attached.
     */
    
    public DifferentialSubsystem(String name, int sidecarNum, int leftPort,
            int rightPort) {
        
        System.out.println("Creating DifferentialSubsystem " + name + "...");
        subsystemName = name;
        
        leftShifterVictor = new Victor(sidecarNum, leftPort);
        rightShifterVictor = new Victor(sidecarNum, rightPort);
        
        setDifferentialState(DifferentialState.LOW_GEAR);
        
    } // end Differential Subsystem Constructor
    
    
    // METHODS //
    
    
    // There is no default command for this subsystem.
    public void initDefaultCommand() {
        //setDefaultCommand(new TestableCommand());
    } // end initDefaultCommand
    
    /**
     * Changes the state of both shifters on the robot.
     * 
     * @param state A DifferentialState. 
     */
    
    public final void setDifferentialState(DifferentialState state) {
        
        leftShifterVictor.set(state.getValue());
        rightShifterVictor.set(state.getValue());
     
        currentState = state;
        
    } // end changeDifferentialState
    
    public void onlyWorkTheRightSide(double input) {
        rightShifterVictor.set(input);
    } // end onlyWorkTheRightSide
    
    /**
     * Grabs the current state of the DifferentialSubsystem
     * @return 
     */
    public DifferentialState getDifferentialState() {
        return currentState;
    } // end getDifferentialState
    
} // end DifferentialSubsystem
